#include <rclcpp/rclcpp.hpp>
#include <rcl_interfaces/msg/parameter_event.hpp>
#include <variant>
#include <sstream>

class ParameterSubscriber : public rclcpp::Node {
public:
    ParameterSubscriber() : Node("parameter_subscriber") {
        // 订阅参数事件
        parameter_subscription_ = this->create_subscription<rcl_interfaces::msg::ParameterEvent>(
            "/parameter_events", 10, std::bind(&ParameterSubscriber::parameter_callback, this, std::placeholders::_1));
    }

private:
    void parameter_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr msg) {
        RCLCPP_INFO(this->get_logger(), "Parameter event received for node: %s", msg->node.c_str());

        if (msg->node == "amcl") {
            for (const auto &changed_parameter : msg->changed_parameters) {
                RCLCPP_INFO(this->get_logger(), "Node: %s, Parameter: %s, Value: %s",
                            msg->node.c_str(),
                            changed_parameter.name.c_str(),
                            to_string(changed_parameter.value).c_str());
            }

            for (const auto &new_parameter : msg->new_parameters) {
                RCLCPP_INFO(this->get_logger(), "Node: %s, New Parameter: %s, Value: %s",
                            msg->node.c_str(),
                            new_parameter.name.c_str(),
                            to_string(new_parameter.value).c_str());
            }

            for (const auto &deleted_parameter : msg->deleted_parameters) {
                RCLCPP_INFO(this->get_logger(), "Node: %s, Deleted Parameter: %s",
                            msg->node.c_str(),
                            deleted_parameter.name.c_str());
            }
        }
    }

    std::string to_string(const rcl_interfaces::msg::ParameterValue &value) {
        switch (value.type) {
            case rcl_interfaces::msg::ParameterType::PARAMETER_BOOL:
                return value.bool_value ? "true" : "false";
            case rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER:
                return std::to_string(value.integer_value);
            case rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE:
                return std::to_string(value.double_value);
            case rcl_interfaces::msg::ParameterType::PARAMETER_STRING:
                return value.string_value;
            case rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY:
                return "[byte array]";
            case rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY:
                return "[bool array]";
            case rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY:
                return "[integer array]";
            case rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY:
                return "[double array]";
            case rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY:
                return "[string array]";
            default:
                return "unknown type";
        }
    }

    rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr parameter_subscription_;
};

int main(int argc, char * argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<ParameterSubscriber>());
    rclcpp::shutdown();
    return 0;
}